


#include "motorcontrol.h"
#include <Arduino.h>
#include <Wire.h>



// Define the control inputs
#define MOT_F2_PIN A3
#define MOT_F1_PIN A2

#define MOT_E2_PIN A1
#define MOT_E1_PIN A0

#define MOT_D2_PIN 11
#define MOT_D1_PIN 10

#define MOT_C2_PIN 9
#define MOT_C1_PIN 8

#define MOT_B2_PIN 7
#define MOT_B1_PIN 6

#define MOT_A2_PIN 5
#define MOT_A1_PIN 4


void set_motor(void)
{
  // Set all the motor control inputs to OUTPUT
  pinMode(MOT_A1_PIN, OUTPUT);
  pinMode(MOT_A2_PIN, OUTPUT);
  pinMode(MOT_B1_PIN, OUTPUT);
  pinMode(MOT_B2_PIN, OUTPUT);
  pinMode(MOT_C1_PIN, OUTPUT);
  pinMode(MOT_C2_PIN, OUTPUT);
  pinMode(MOT_D1_PIN, OUTPUT);
  pinMode(MOT_D2_PIN, OUTPUT);
  pinMode(MOT_E1_PIN, OUTPUT);
  pinMode(MOT_E2_PIN, OUTPUT);
  pinMode(MOT_F1_PIN, OUTPUT);
  pinMode(MOT_F2_PIN, OUTPUT);

  // Turn off motors - Initial state
  digitalWrite(MOT_A1_PIN, LOW);
  digitalWrite(MOT_A2_PIN, LOW);
  digitalWrite(MOT_B1_PIN, LOW);
  digitalWrite(MOT_B2_PIN, LOW);
  digitalWrite(MOT_C1_PIN, LOW);
  digitalWrite(MOT_C2_PIN, LOW);
  digitalWrite(MOT_D1_PIN, LOW);
  digitalWrite(MOT_D2_PIN, LOW);
  digitalWrite(MOT_E1_PIN, LOW);
  digitalWrite(MOT_E2_PIN, LOW);
  digitalWrite(MOT_F1_PIN, LOW);
  digitalWrite(MOT_F2_PIN, LOW);
 
//   // Initialize the serial UART at 9600 baud
  
   
}
 

 
/// Set the current on a motor channel using PWM and directional logic.
///
/// \param pwm    PWM duty cycle ranging from -255 full reverse to 255 full forward
/// \param IN1_PIN  pin number xIN1 for the given channel
/// \param IN2_PIN  pin number xIN2 for the given channel

void set_motor_pwm(int pwm, int IN1_PIN, int IN2_PIN)
{
  if (pwm == 0) {  // reverse speeds
    digitalWrite(IN1_PIN, LOW);
    digitalWrite(IN2_PIN, LOW);
 
  } else if(pwm > 0){ 
    digitalWrite(IN1_PIN, LOW);
    analogWrite(IN2_PIN, pwm);
  }
  else{
        if (pwm < -255){ pwm = -255; }
            digitalWrite(IN1_PIN, HIGH);
            analogWrite(IN2_PIN,255+pwm);
      
  }
}




 
/// Set the current on both motors.
///
/// \param pwm_A  motor A PWM, -255 to 255
/// \param pwm_B  motor B PWM, -255 to 255
void set_motor_currents(int pwm_A, int pwm_B, int pwm_C, int pwm_D, int pwm_E, int pwm_F)
{
  set_motor_pwm(pwm_A, MOT_A1_PIN, MOT_A2_PIN);
  set_motor_pwm(pwm_B, MOT_B1_PIN, MOT_B2_PIN);
  set_motor_pwm(pwm_C, MOT_C1_PIN, MOT_C2_PIN);
  set_motor_pwm(pwm_D, MOT_D1_PIN, MOT_D2_PIN);
  set_motor_pwm(pwm_E, MOT_E1_PIN, MOT_E2_PIN);
  set_motor_pwm(pwm_F, MOT_F1_PIN, MOT_F2_PIN);

}

